#include <mex.h>
#include <vector>
#include <iostream>
#include <math.h>
using namespace std;
void mexFunction(int          nlhs,
		 mxArray      *plhs[],
		 int          nrhs,
         mxArray *prhs[]=NULL)
{ 
	#define B_OUT plhs[0]
	#define A_IN prhs[0]

    double* tip0= (double *) mxGetPr(A_IN);
    int xrad,yrad,zrad;
    if(nrhs<=3){
         xrad=5;
         yrad=3;
         zrad=3;
    }
    else{
         double* tip_size=(double *) mxGetPr(prhs[3]);
         xrad=tip_size[0];
         yrad=tip_size[1];
         zrad=tip_size[2];
    }
    int needle_num;
     if(nrhs<=2)
         needle_num=2;
     else{
         double* needle_number=(double *) mxGetPr(prhs[2]);
         needle_num=needle_number[0];
     }
    std::vector<double>  Xcor;
    std::vector<double>  Ycor;
    std::vector<double>  Zcor;

    for(double i=0;i<=90;i=i+10)
    {
        double phi=((3.14159)/180)*i;
        for(double j=0 ;j<90;j=j+10)
        {
              double theta,xprim,yprim,zprim;

              theta=((3.14159)/180)*j;
              xprim=int(xrad*cos(phi)*cos(theta));
              yprim=int(yrad*sin(theta)*cos(phi));
              zprim=int(zrad*sin(phi));

              Xcor.push_back(xprim);
              Ycor.push_back(-yprim);
              Zcor.push_back(zprim);
              Xcor.push_back(xprim);
              Ycor.push_back(yprim);
              Zcor.push_back(-zprim);


              Xcor.push_back(xprim);
              Ycor.push_back(-yprim);
              Zcor.push_back(-zprim);
              Xcor.push_back(xprim);
              Ycor.push_back(yprim);
              Zcor.push_back(zprim);

              Xcor.push_back(-xprim);
              Ycor.push_back(-yprim);
              Zcor.push_back(zprim);
              Xcor.push_back(-xprim);
              Ycor.push_back(yprim);
              Zcor.push_back(zprim);

              Xcor.push_back(-xprim);
              Ycor.push_back(-yprim);
              Zcor.push_back(-zprim);
              Xcor.push_back(-xprim);
              Ycor.push_back(yprim);
              Zcor.push_back(-zprim);
        }
    }
    int dummy=0;
    double pi=3.14159;
    int F = 1;
    int P={Xcor.size()};
	// needle 1
    mxArray *amir0;
    amir0= mxCreateDoubleMatrix(F,P, mxREAL);
    double *row = mxGetPr(amir0);
    mxArray *amir1;
    amir1= mxCreateDoubleMatrix(F,P, mxREAL);
    double *col = mxGetPr(amir1);
    mxArray *amir2;
    amir2= mxCreateDoubleMatrix(F,P, mxREAL);
    double *height = mxGetPr(amir2);
	// needle 2
    mxArray *amir10;
    amir10= mxCreateDoubleMatrix(F,P, mxREAL);
    double *row1 = mxGetPr(amir10);
    mxArray *amir11;
    amir11= mxCreateDoubleMatrix(F,P, mxREAL);
    double *col1 = mxGetPr(amir11);
    mxArray *amir12;
    amir12= mxCreateDoubleMatrix(F,P, mxREAL);
    double *height1 = mxGetPr(amir12);
    double *row2,*row3,*row4,*col2,*col3,*col4,*height2,*height3,*height4;
    if (needle_num >= 3) {//needle 3
        mxArray *amir20;
        amir20= mxCreateDoubleMatrix(F,P, mxREAL);
        row2 = mxGetPr(amir20);
        mxArray *amir21;
        amir21= mxCreateDoubleMatrix(F,P, mxREAL);
        col2 = mxGetPr(amir21);
        mxArray *amir22;
        amir22= mxCreateDoubleMatrix(F,P, mxREAL);
        height2 = mxGetPr(amir22);
    }
    if (needle_num >= 4) {//needle 4
        mxArray *amir30;
        amir30= mxCreateDoubleMatrix(F,P, mxREAL);
        row3 = mxGetPr(amir30);
        mxArray *amir31;
        amir31= mxCreateDoubleMatrix(F,P, mxREAL);
        col3 = mxGetPr(amir31);
        mxArray *amir32;
        amir32= mxCreateDoubleMatrix(F,P, mxREAL);
        height3 = mxGetPr(amir32);
    }
    if (needle_num >= 5) {//needle 5
        mxArray *amir40;
        amir40= mxCreateDoubleMatrix(F,P, mxREAL);
        row4 = mxGetPr(amir40);
        mxArray *amir41;
        amir41= mxCreateDoubleMatrix(F,P, mxREAL);
        col4 = mxGetPr(amir41);
        mxArray *amir42;
        amir42= mxCreateDoubleMatrix(F,P, mxREAL);
        height4 = mxGetPr(amir42);
    }
    for (std::vector<double>::iterator it = Xcor.begin(); it != Xcor.end(); ++it,dummy++){
		// needle 1
        row[dummy]=int(Xcor[dummy]*cos(tip0[3]*pi/180.0)+Ycor[dummy]*sin(tip0[4]*pi/180.0)*sin(tip0[3]*pi/180.0)-Zcor[dummy]*cos(tip0[4]* pi/180.0)*cos(tip0[3]*pi/180.0))+tip0[0];
        col[dummy]=int(Ycor[dummy]*cos(tip0[4] * pi / 180.0)+ Zcor[dummy]*sin(tip0[4] * pi / 180.0))+tip0[1];
        height[dummy]=int(Xcor[dummy]*sin(tip0[3]*pi/180.0)-Ycor[dummy]*sin(tip0[4]*pi/180.0)*cos(tip0[3]*pi/180.0)+Zcor[dummy]*cos(tip0[4]*pi/180.0)*cos(tip0[3]*pi/180.0))+tip0[2];
		// needle 2
        if (needle_num >= 2){
        row1[dummy]=int(Xcor[dummy]*cos(tip0[8]*pi/180.0)+Ycor[dummy]*sin(tip0[9]*pi/180.0)*sin(tip0[8]*pi/180.0)-Zcor[dummy]*cos(tip0[9]* pi/180.0)*cos(tip0[8]*pi/180.0))+tip0[5];
        col1[dummy]=int(Ycor[dummy]*cos(tip0[9] * pi / 180.0)+ Zcor[dummy]*sin(tip0[9] * pi / 180.0))+tip0[6];
        height1[dummy]=int(Xcor[dummy]*sin(tip0[8]*pi/180.0)-Ycor[dummy]*sin(tip0[9]*pi/180.0)*cos(tip0[8]*pi/180.0)+Zcor[dummy]*cos(tip0[9]*pi/180.0)*cos(tip0[8]*pi/180.0))+tip0[7];
        }
//		// needle 3
        if (needle_num >= 3) {
        row2[dummy]=int(Xcor[dummy]*cos(tip0[12]*pi/180.0)+Ycor[dummy]*sin(tip0[13]*pi/180.0)*sin(tip0[12]*pi/180.0)-Zcor[dummy]*cos(tip0[13]* pi/180.0)*cos(tip0[12]*pi/180.0))+tip0[10];
        col2[dummy]=int(Ycor[dummy]*cos(tip0[14] * pi / 180.0)+ Zcor[dummy]*sin(tip0[14] * pi / 180.0))+tip0[11];
        height2[dummy]=int(Xcor[dummy]*sin(tip0[13]*pi/180.0)-Ycor[dummy]*sin(tip0[14]*pi/180.0)*cos(tip0[13]*pi/180.0)+Zcor[dummy]*cos(tip0[14]*pi/180.0)*cos(tip0[13]*pi/180.0))+tip0[12];
        }
        // needle 4
        if (needle_num >= 4) {
            row3[dummy]=int(Xcor[dummy]*cos(tip0[17]*pi/180.0)+Ycor[dummy]*sin(tip0[18]*pi/180.0)*sin(tip0[17]*pi/180.0)-Zcor[dummy]*cos(tip0[18]* pi/180.0)*cos(tip0[17]*pi/180.0))+tip0[15];
            col3[dummy]=int(Ycor[dummy]*cos(tip0[19] * pi / 180.0)+ Zcor[dummy]*sin(tip0[19] * pi / 180.0))+tip0[16];
            height3[dummy]=int(Xcor[dummy]*sin(tip0[18]*pi/180.0)-Ycor[dummy]*sin(tip0[19]*pi/180.0)*cos(tip0[18]*pi/180.0)+Zcor[dummy]*cos(tip0[19]*pi/180.0)*cos(tip0[18]*pi/180.0))+tip0[17];
        }
        // needle 5
        if (needle_num >= 5) {
            row4[dummy]=int(Xcor[dummy]*cos(tip0[22]*pi/180.0)+Ycor[dummy]*sin(tip0[23]*pi/180.0)*sin(tip0[22]*pi/180.0)-Zcor[dummy]*cos(tip0[23]* pi/180.0)*cos(tip0[22]*pi/180.0))+tip0[20];
            col4[dummy]=int(Ycor[dummy]*cos(tip0[24] * pi / 180.0)+ Zcor[dummy]*sin(tip0[24] * pi / 180.0))+tip0[21];
            height4[dummy]=int(Xcor[dummy]*sin(tip0[23]*pi/180.0)-Ycor[dummy]*sin(tip0[24]*pi/180.0)*cos(tip0[23]*pi/180.0)+Zcor[dummy]*cos(tip0[24]*pi/180.0)*cos(tip0[23]*pi/180.0))+tip0[22];
        }
    }
    double Ta=37, Qm=4200, wb=0.0005, rb=1000, cb=3.6, Tc=37, h0=10, k=2, L=.03, Tf=20;
    double Tinitial;
    const int dim=70;

    double xl[dim];
    for(int i=0;i <=dim;i++)			 //initialize x array
        xl[i]=L*i/dim;				    //to cover all x=0~L and it depends on L value
    std::vector<double>  p1D;
    std::vector<std::vector<double> > p2D;
    std::vector<std::vector<std::vector<double> > > p3D;
    std::vector<std::vector<std::vector<std::vector<double> > > > p4D;

    for(int x=1;x<=dim;x++){
        double A=wb*rb*cb/k;
        double p1=sqrt(A)*cosh(sqrt(A)*xl[x])+(h0/k)*sinh(sqrt(A)*xl[x]);
        double p2=Qm/(wb*rb*cb);
        double p3=(sqrt(A)*cosh(sqrt(A)*L)+(h0/k)*sinh(sqrt(A)*L));
        p2D.clear();
            for(int y=1;y<=dim;y++){
                p1D.clear();
                for(int z=1;z<=dim;z++){
                    Tinitial=Ta+p2+((Tc-Ta-p2)*p1)/p3+((h0/k)*(Tf-Ta-p2)*sinh(sqrt(A)*(L-xl[x])))/p3;
                    p1D.push_back(Tinitial);
//							cout<<"  "<<p1D[z];
                }
                p2D.push_back(p1D);
//					cout<<endl;
            }
            p3D.push_back(p2D);
//					cout<< endl;
    }
    p4D.push_back(p3D);

    //	//Calculates isotherms for 4D (3D and Time)
//	/*   ------------------------------------------  this equation is taken from formula 11 of paper "NUMERICAL SIMULATION OF 3-D
//	FREEZING AND HEATING PROBLEMS FOR COMBINED CRYOSURGERY AND HYPERTHERMIA THERAPY" ------------------------------------------------------------------- */
double final;
    if (nrhs<=1)
        final=100;
    else
    {
        double* time= (double *) mxGetPr(prhs[1]);
        final = time[0];
    }
            double W,Beta=.5,m=6,Deltat=1,Deltax=.1,Cb=3600,Fo,Qhatm=0,khat=2 ,Chat=3600,whatb=0;
                        double c1, c2,c3,c4;
                        double X,Y,Z;
                        W = Cb*whatb/Chat;
                        c1=1+W*Beta*Deltat;
                        Fo = khat*Deltat/(Chat*pow(Deltax,2));
                        for(int t=0;t<final;t++){
                                p3D.clear();
                                for(int x=1;x<=dim-2;x++){
                                    //X=ceil((tip1[0]-origin1[0])/spacing[1])-dim/2+x;
                                    p2D.clear();
                                    for(int y=1;y<=dim-2;y++){
                                        //Y=ceil((tip1[1]-origin1[1])/spacing[1])-dim/2+y;
                                        p1D.clear();
                                        p1D.push_back(p4D[t][x][y][1]);  // add point in the beginning
                                        for(int z=1;z<=dim-2;z++){
//											if(p4D[t][x][y][z]<-8)
//											{
//												Qhatm=0;khat=0.5;Chat=1.8; whatb=0;
//											}
//											else if(p4D[t][x][y][z]<=-1 && p4D[t][x][y][z]>=-8)
//											{
//												Qhatm=0,khat=1.25 ,Chat=38410,whatb=0;
//											}
//											else
//											{
//												Qhatm=0,khat=2 ,Chat=3600,whatb=0;
//											}
//											W = Cb*whatb/Chat;
//											c1=1+W*Beta*Deltat;
//											Fo = khat*Deltat/(Chat*pow(Deltax,2));

                                            c2 = (Fo/c1)*(p4D[t][x+1][y][z]+p4D[t][x][y+1][z]+p4D[t][x][y][z+1]);
                                            c3 = (Fo/c1)*(p4D[t][x-1][y][z]+p4D[t][x][y-1][z]+p4D[t][x][y][z-1]);
                                            c4 = (1-W*(1-Beta)*Deltat-m*Fo)*p4D[t][x][y][z]/c1+c2+c3+(Qhatm+whatb*Cb*Ta)*Deltat/c1;
//                                            if(z<=tip0[2]+5 && z>=(tip0[2]-5) ){                 //put the correct position of needles here
//                                                if(y<=tip0[1]+3 && y>=tip0[1]-3 ){
//                                                    if(x<=tip0[0]+3 && x>=tip0[0]-3)
//                                                        c4=-196;
//                                                }
//                                            }
//                                            if(z<=tip0[5]+5 && z>=(tip0[5]-5) ){                 //put the correct position of needles here
//                                                if(y<=tip0[4]+3 && y>=tip0[4]-3 ){
//                                                    if(x<=tip0[3]+3 && x>=tip0[3]-3)
//                                                        c4=-196;
//                                                }
//                                            }

//											if (needle_prism[x][y][z]==1)
                                            //Z=ceil((tip1[2]-origin1[2])/spacing[2])-dim/2+z;
//											Z=ceil((tip[2]-(dim/2)*spacing[2]-origin[2])/spacing[2])+z; //change coordinates to bioheatimage coordinates(centered at needle tip)
                                            //this situation is different with two above
//											float distance= sqrt(pow(p[0]-tip[0],2)+pow(p[1]-tip[1],2)+pow(p[2]-tip[2],2));
//											if(t<1500 || t>2000){
//												double p[3];
//												p[0]=tip0[0]-(dim/2-x)*spacing[0];
//												p[1]=tip0[1]-(dim/2-y)*spacing[1];
//												p[2]=tip0[2]-(dim/2-z)*spacing[2];
												
                                                //if(m_cylinder0->IsInside (p) | m_cylinder1->IsInside (p) | m_cylinder2->IsInside (p))
                                                //		c4=-196;
//											}
//											if(z<=dim/3+3 && z>=dim/3+1)                 //put the correct position of vessels here
//												if(y<=dim/4+1 && y>=dim/4-1)
//													if(x<=dim/3+25 && x>=dim/3-7)
                                                                                 //change coordinates to the vessel image coordinates


//											Z=ceil((tip[2]-origin[2])/spacing[2])-dim/2+z;
//											Z=ceil((tip[2]-(dim/2)*spacing[2]-origin[2])/spacing[2])+z; //this situation is different with two above
//											if(X<dim1[0]+traslation[0] && X>traslation[0]){
//												if(Y<dim1[1]+traslation[1] && Y>traslation[1]){
//													if(Z<dim1[2]+traslation[2] && Z>traslation[2]){
//														if(outData->GetScalarComponentAsFloat ( X,Y,Z,0)==255){
//															c4=37;
//														}
//													}
//												}
//											}
                                            p1D.push_back(c4);
//											cout<<"  "<<p1D[z];
                                        }
                                        p1D.push_back(p4D[t][x][y][dim-2]); //add point in the end
                                        if(y==1)
                                            p2D.push_back(p1D);             //add 1D in the beginning
                                        p2D.push_back(p1D);
            //							cout << " " <<p4D[t][x][y+1][1];
//										cout<<endl;
                                    }
                                    p2D.push_back(p1D);                    // add 1D in the end
                                    if(x==1)
                                        p3D.push_back(p2D);                //add 2D in the beginning
                                    p3D.push_back(p2D);
//									cout<<endl;
                                }
//								cout<<"done"<<endl;
                                p3D.push_back(p2D);                       // add 2D in the end
                                double n1,n2,n3,n4,n5,n6,n7,n8,n9,n10,n11,n12;
                                int i=0;
                                for (std::vector<double>::iterator it = Xcor.begin(); it != Xcor.end(); ++it,i++){
                                    if(t<600 || t>900){
                                        n1=(double )row[i];
                                        n2=(double )col[i];
                                        n3=(double )height[i];
                                        p3D[n1][n2][n3]=-196;

                                        if(needle_num >= 2){
                                            n4=(double )row1[i];
                                            n5=(double )col1[i];
                                            n6=(double )height1[i];
                                            p3D[n4][n5][n6]=-196;
                                        }
                                        if(needle_num >= 3){
                                            n7=(double )row2[i];
                                            n8=(double )col2[i];
                                            n9=(double )height2[i];
                                            p3D[n7][n8][n9]=-196;
                                        }
                                        if(needle_num >= 4){
                                            n7=(double )row3[i];
                                            n8=(double )col3[i];
                                            n9=(double )height3[i];
                                            p3D[n7][n8][n9]=-196;
                                        }
                                        if(needle_num >= 5){
                                            n7=(double )row4[i];
                                            n8=(double )col4[i];
                                            n9=(double )height4[i];
                                            p3D[n7][n8][n9]=-196;
                                        }
                                    }
                                }
                                    if(nrhs>=5){
//                                        int j=0;
                                         double* vesselx=(double *) mxGetPr(prhs[4]);
                                         float D = mxGetM(prhs[4]);
                                         double* vessely=(double *) mxGetPr(prhs[5]);
                                         double* vesselz=(double *) mxGetPr(prhs[6]);

                                         for (int j=0;j<D;j++) {
                                         n10=vesselx[j];
                                         n11=vessely[j];
                                         n12=vesselz[j];
                                         p3D[n10][n11][n12]=37;
                                         }
                                    }
//                                    mexPrintf("value2 %f \n", n3);

                                p4D.push_back(p3D);
                            }
                        p4D.clear();

//						//int K = mxGetNumberOfDimensions(p3D);
//
//						//const int *N = mxGetDimensions(p3D);
                        int K = 3;
                        int N[]={dim,dim,dim};
                        B_OUT = mxCreateNumericArray(K, N, mxDOUBLE_CLASS, mxREAL);
                        double *B = mxGetPr(B_OUT);
                        for(int n=0;n<N[0];n++){
                            for(int m=0;m<N[1];m++)
                                for(int k=0;k<N[2];k++){
                                    //B[k+m*N[2] + N[1]*N[2]*n] = A[k+m*N[2] + N[1]*N[2]*n];
//                                    if (p3D[n][m][k]<=0)
//                                        p3D[n][m][k]=1;
//                                    else
//                                        p3D[n][m][k]=0;
                                    B[k+m*N[2] + N[1]*N[2]*n] = p3D[n][m][k];
                                }
                        }
return;
} 
